//
//  GeometryCalculation.m
//  Portable Traffic Signal Assistor
//
//  Created by Hao Xu on 6/1/12.
//  Copyright (c) 2012 TTU. All rights reserved.
//

#import "GeometryCalculation.h"

@implementation GeometryCalculation

- (Signal *)calculateNextSignal:(NSArray *)signalList newHeading:(float)newHeading coordinate:(CLLocationCoordinate2D)coordinate agency:(Agency *)agency subSystem:(SubSystem *)subSystem
{
    
//    CLLocationManager *locationManager = [[CLLocationManager alloc] init];
//    locationManager.delegate = self; 
//    locationManager.desiredAccuracy = kCLLocationAccuracyBest; 
//    locationManager.distanceFilter = kCLDistanceFilterNone; 
//    [locationManager startUpdatingLocation];
//    [locationManager startUpdatingHeading];
//    
//    CLLocation *location = [locationManager location];
////    CLLocationCoordinate2D user = [location coordinate];
//    CLLocationCoordinate2D coordinate=[location coordinate];
    if (coordinate.latitude==0) {
        return nil;
    }
    NSLog(@"!!!!! Heading : %f\n",newHeading);
    Signal *selectedSignal=nil;
    float minimumDistance=1000;
    for (int i = 0; i<signalList.count; i++) {
        Signal *signal=[signalList objectAtIndex:i];
        Intersection *intersection=[XMLParser getIntersection:signal.signalID agencyID:agency.agencyID subSystemID:subSystem.subSystemID];
        if (intersection.intersectionCoordinate.latitude==0) {
            continue;
        }
        CLLocationCoordinate2D signalCoordinate=intersection.intersectionCoordinate;
        float signalDirection=[GeometryCalculation calculateUserAngle:coordinate signal:signalCoordinate];
        float heading = newHeading; //in degrees
        float headingDifference=fabsf(heading-signalDirection);
        if (headingDifference>180) {
            headingDifference=360-180;
        }
       
        if (headingDifference<=15) {
             NSLog(@"!!!!! signalDirection : %f\n",signalDirection);
            float distance=[GeometryCalculation distanceOfTwoCoordinates:coordinate coordB:signalCoordinate];
            if (minimumDistance>distance) {
                minimumDistance=distance;
                selectedSignal=signal;
            }
        }
    }
    return selectedSignal;
}
+ (Link *) getlinkBetweenTwoIntersections:(NSString *)intersection1ID intersection2ID:(NSString *)intersection2ID subSystemID:(NSString *)subSystemID agencyID:(NSString *)agencyID

{
    NSArray *linklist=[XMLParser getLinkListFromIntersection:agencyID subSystemID:subSystemID intersectionID:intersection1ID];
    for (Link * link in linklist) {
        if ([link.toIntersectionID isEqualToString:intersection2ID]) {
            return link;
        }
    }
    linklist=[XMLParser getLinkListToIntersection:agencyID subSystemID:subSystemID intersectionID:intersection1ID];
    for (Link * link in linklist) {
        if ([link.fromIntersectionID isEqualToString:intersection2ID]) {
            return link;
        }
    }
    return nil;
}
+ (Signal *)calculateNextSignal:(Signal *)signal heading:(float)heading  agency:(Agency *)agency subSystem:(SubSystem *)subSystem timingPattern:(TimingPattern *)timingPattern
{
    Intersection * intersection=[XMLParser getIntersection:signal.signalID agencyID:agency.agencyID subSystemID:subSystem.subSystemID];
    if (intersection.intersectionCoordinate.latitude==0) {
        return nil;
    }
    NSArray *linkList=[XMLParser getLinkListFromIntersection:agency.agencyID subSystemID:subSystem.subSystemID intersectionID:signal.signalID];
    for (Link *link in linkList) {
        Intersection *nextIntersection=[XMLParser getIntersection:link.toIntersectionID agencyID:agency.agencyID subSystemID:subSystem.subSystemID];
        if (nextIntersection.intersectionCoordinate.latitude==0) {
            continue;
        }
        float direction=[GeometryCalculation calculateUserAngle:intersection.intersectionCoordinate signal:nextIntersection.intersectionCoordinate];
        if (fabs(direction-heading)<90) {
            Signal *nextSignal=[XMLParser getSignal:nextIntersection.intersectionID agencyID:agency.agencyID subSystemID:subSystem.subSystemID timingPatternID:timingPattern.timingPatternID];
            return nextSignal;
        }
        if (fabs(direction-heading)>270) {
            Signal *nextSignal=[XMLParser getSignal:nextIntersection.intersectionID agencyID:agency.agencyID subSystemID:subSystem.subSystemID timingPatternID:timingPattern.timingPatternID];
            return nextSignal;
        }
       
    }
    return nil;
    
}

+ (int)calculateNextSignalIndex:(NSArray *)signalList newHeading:(float)newHeading coordinate:(CLLocationCoordinate2D)coordinate agency:(Agency *)agency subSystem:(SubSystem *)subSystem
{
    
    //    CLLocationManager *locationManager = [[CLLocationManager alloc] init];
    //    locationManager.delegate = self;
    //    locationManager.desiredAccuracy = kCLLocationAccuracyBest;
    //    locationManager.distanceFilter = kCLDistanceFilterNone;
    //    [locationManager startUpdatingLocation];
    //    [locationManager startUpdatingHeading];
    //
    //    CLLocation *location = [locationManager location];
    ////    CLLocationCoordinate2D user = [location coordinate];
    //    CLLocationCoordinate2D coordinate=[location coordinate];
    if (coordinate.latitude==0) {
        return -1;
    }
    NSLog(@"!!!!! Heading : %f\n",newHeading);
    int nextSignalIndex=-1;
    float minimumDistance=1000;
    for (int i = 0; i<signalList.count; i++) {
        Signal *signal=[signalList objectAtIndex:i];
        Intersection *intersection=[XMLParser getIntersection:signal.signalID agencyID:agency.agencyID subSystemID:subSystem.subSystemID];
        if (intersection.intersectionCoordinate.latitude==0) {
            continue;
        }
        CLLocationCoordinate2D signalCoordinate=intersection.intersectionCoordinate;
        if (coordinate.latitude==signalCoordinate.latitude && coordinate.longitude==signalCoordinate.longitude) {
            continue;
        }
        float signalDirection=[GeometryCalculation calculateUserAngle:coordinate signal:signalCoordinate];
        float heading = newHeading; //in degrees
        float headingDifference=fabsf(heading-signalDirection);
        if (headingDifference>180) {
            headingDifference=360-180;
        }
//        {
//            UIAlertView *alert = [[UIAlertView alloc] initWithTitle:@"Heading"
//                                                            message:[NSString stringWithFormat:@"heading=%f,signaldirection=%f",heading,signalDirection]
//                                                           delegate:self
//                                                  cancelButtonTitle:@"YES"
//                                                  otherButtonTitles:@"NO",nil];
//            [alert show];
//            [alert release];
//        }
        if (headingDifference<=30) {
            NSLog(@"!!!!! signalDirection : %f\n",signalDirection);
            float distance=[GeometryCalculation distanceOfTwoCoordinates:coordinate coordB:signalCoordinate];
            if (minimumDistance>distance) {
                minimumDistance=distance;
                nextSignalIndex=i;
            }
        }
    }
    return nextSignalIndex;
}



+(float) calculateUserAngle:(CLLocationCoordinate2D)user signal:(CLLocationCoordinate2D)signal
{
    float locLat = signal.latitude;
    float locLon = signal.longitude;
    float degrees=0;
      NSLog(@"%f ; %f", locLat, locLon);
    
      float pLat = 0.0;
      float pLon = 0.0;
    if (locLon==user.longitude) {
        if (locLat>=user.latitude) {
            return 0;
        }else{
            return 180;
        }
    }
    if (locLat==user.latitude) {
        if (locLon<=user.longitude) {
            return 270;
        }else
        {
            return 90;
        }
    }
      if(locLat > user.latitude && locLon > user.longitude) {
            // north east
        
            pLat = user.latitude;
            pLon = locLon;
          degrees=0;
      }
      else if(locLat > user.latitude && locLon < user.longitude) {
            // north west
        
            pLat = locLat;
            pLon = user.longitude;
        
            degrees = 270;
          
      }
      else if(locLat < user.latitude && locLon < user.longitude) {
            // south west
        
            pLat = user.latitude;
            pLon = locLon;
        
            degrees = 180;
          
//          return degrees;
          }
      else if(locLat < user.latitude && locLon > user.longitude) {
            // south east
        
            pLat = locLat;
            pLon = user.longitude;
        
            degrees = 90;
//          return degrees;
          }
    CLLocationCoordinate2D pointCoordinate={pLat,pLon};
    
    // Vector QP (from user to point)
    float distanceU2P = [GeometryCalculation distanceOfTwoCoordinates:user coordB:pointCoordinate];
    
    
    // Vector QL (from user to location)
    float distanceU2S = [GeometryCalculation distanceOfTwoCoordinates:user coordB:signal];
    float sinDegrees=1;
    if (distanceU2S>0) {
        sinDegrees=distanceU2P/distanceU2S;
    }
    degrees = degrees + asin(sinDegrees)*180/M_PI;
//    if (degrees>=360) {
//        degrees=degrees-360;
//    }
    return degrees;
      // degrees between QP and QL

      
    
}

+ (float) distanceOfTwoCoordinates:(CLLocationCoordinate2D)coordA coordB:(CLLocationCoordinate2D)coordB
{
    CLLocation *locA = [[[CLLocation alloc] initWithLatitude:coordA.latitude longitude:coordA.longitude] autorelease];
    
    CLLocation *locB = [[[CLLocation alloc] initWithLatitude:coordB.latitude longitude:coordB.longitude] autorelease];
    
    CLLocationDistance distance = [locA distanceFromLocation:locB];
    return distance;
}

@end
